{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import cv2\n",
    "import numpy as np\n",
    "import math\n",
    "import serial\n",
    "\n",
    "def Routing(point1,point2,remap_point,image):\n",
    "    p1 = remap_point[point1[0]][point1[1]]\n",
    "    p2 = remap_point[point2[0]][point2[1]]\n",
    "    #cv2.circle(im_crop, p1, 2, (0, 0, 255), 2)  # 画圆心\n",
    "    #cv2.circle(im_crop, p2, 2, (0, 0, 255), 2)  # 画圆心\n",
    "    #如果x轴相等，那就是y轴需要移动\n",
    "    if(p1[0]==p2[0]):\n",
    "            if p2[1]>p1[1]:\n",
    "                a = image[p1[1] :p2[1],p1[0] ]> 0\n",
    "            else:\n",
    "                a = image[p2[1]:p1[1], p1[0]] > 0\n",
    "            sum = np.sum(a)\n",
    "            if sum>0 :\n",
    "                return False\n",
    "    if (p1[1] == p2[1]):\n",
    "        if(p1[0]>p2[0]):\n",
    "            a = image[p1[1], p2[0]:p1[0]] > 0\n",
    "        else:\n",
    "            a = image[p1[1], p1[0]:p2[0]] > 0\n",
    "        sum = np.sum(a)\n",
    "        if sum > 0:\n",
    "            return False\n",
    "    return True\n",
    "def get_route(point0,point1,im_crop,edge,points):\n",
    "    bianli_list = []\n",
    "    route = []\n",
    "    head = 0#队列头\n",
    "   # rear = 0#队列尾巴\n",
    "    bianli_list.append([point0[0],point0[1],-1,1])\n",
    "    while bianli_list[head][0]!=point1[0] or bianli_list[head][1]!=point1[1]:\n",
    "        x = bianli_list[head][0]\n",
    "        y = bianli_list[head][1]\n",
    "        r = bianli_list[head][3]\n",
    "        point_lf = [x-1,y]\n",
    "        point_rg = [x + 1, y]\n",
    "        point_up = [x, y-1]\n",
    "        point_dn = [x, y+1]\n",
    "        if 0<=point_lf[0] < 10 and 0<=point_lf[1] < 10:\n",
    "                if point_lf[0] != bianli_list[bianli_list[head][2]][0] or point_lf[1] != bianli_list[bianli_list[head][2]][1]:\n",
    "                    if Routing([x,y],point_lf,points,edge)==True:\n",
    "                        bianli_list.append([point_lf[0],point_lf[1],head,r+1])\n",
    "\n",
    "        if 0 <= point_rg[0] < 10 and 0 <= point_rg[1] < 10:\n",
    "            if point_rg[0] != bianli_list[bianli_list[head][2]][0] or point_rg[1] != bianli_list[bianli_list[head][2]][ 1]:\n",
    "                if Routing([x,y],point_rg,points,edge)==True:\n",
    "                    bianli_list.append([point_rg[0],point_rg[1],head,r+1])\n",
    "\n",
    "        if 0 <= point_up[0] < 10 and 0 <= point_up[1] < 10:\n",
    "            if point_up[0] != bianli_list[bianli_list[head][2]][0] or point_up[1] != bianli_list[bianli_list[head][2]][1]:\n",
    "                if Routing([x,y],point_up,points,edge)==True:\n",
    "                    bianli_list.append([point_up[0],point_up[1],head,r+1])\n",
    "\n",
    "\n",
    "        if 0 <= point_dn[0] < 10 and 0 <= point_dn[1] < 10:\n",
    "            if point_dn[0] != bianli_list[bianli_list[head][2]][0] or point_dn[1] != bianli_list[bianli_list[head][2]][1]:\n",
    "                if Routing([x,y],point_dn,points,edge)==True:\n",
    "                    bianli_list.append([point_dn[0],point_dn[1],head,r+1])\n",
    "        head+=1\n",
    "\n",
    "    while(bianli_list[head][2]!=-1):\n",
    "        x0 = bianli_list[head][0]\n",
    "        y0 = bianli_list[head][1]\n",
    "        route.append([x0,y0])\n",
    "        pre_index = bianli_list[head][2]\n",
    "        x1 = bianli_list[pre_index][0]\n",
    "        y1 = bianli_list[pre_index][1]\n",
    "        cv2.line(im_crop, (points[x0][y0][0],points[x0][y0][1]), (points[x1][y1][0],points[x1][y1][1]), (0, 0, 255), 2)\n",
    "        head = pre_index\n",
    "    return route\n",
    "        # cv2.imshow(\"line\",im_crop)\n",
    "        # cv2.waitKey()\n",
    "def get_short_route(point0,baozang_points,im_crop,edge,points):\n",
    "    bianli_list = []\n",
    "    head = 0#队列头\n",
    "    route = []\n",
    "    total_route = []\n",
    "   # rear = 0#队列尾巴\n",
    "    bianli_list.append([point0[0],point0[1],-1,1])\n",
    "    while(baozang_points):\n",
    "        while [bianli_list[head][0],bianli_list[head][1]] not in baozang_points:\n",
    "            x = bianli_list[head][0]\n",
    "            y = bianli_list[head][1]\n",
    "            r = bianli_list[head][3]\n",
    "            point_lf = [x-1,y]\n",
    "            point_rg = [x + 1, y]\n",
    "            point_up = [x, y-1]\n",
    "            point_dn = [x, y+1]\n",
    "            if 0<=point_lf[0] < 10 and 0<=point_lf[1] < 10:\n",
    "                    if point_lf[0] != bianli_list[bianli_list[head][2]][0] or point_lf[1] != bianli_list[bianli_list[head][2]][1]:\n",
    "                        if Routing([x,y],point_lf,points,edge)==True:\n",
    "                            bianli_list.append([point_lf[0],point_lf[1],head,r+1])\n",
    "\n",
    "            if 0 <= point_rg[0] < 10 and 0 <= point_rg[1] < 10:\n",
    "                if point_rg[0] != bianli_list[bianli_list[head][2]][0] or point_rg[1] != bianli_list[bianli_list[head][2]][ 1]:\n",
    "                    if Routing([x,y],point_rg,points,edge)==True:\n",
    "                        bianli_list.append([point_rg[0],point_rg[1],head,r+1])\n",
    "\n",
    "            if 0 <= point_up[0] < 10 and 0 <= point_up[1] < 10:\n",
    "                if point_up[0] != bianli_list[bianli_list[head][2]][0] or point_up[1] != bianli_list[bianli_list[head][2]][1]:\n",
    "                    if Routing([x,y],point_up,points,edge)==True:\n",
    "                        bianli_list.append([point_up[0],point_up[1],head,r+1])\n",
    "\n",
    "\n",
    "            if 0 <= point_dn[0] < 10 and 0 <= point_dn[1] < 10:\n",
    "                if point_dn[0] != bianli_list[bianli_list[head][2]][0] or point_dn[1] != bianli_list[bianli_list[head][2]][1]:\n",
    "                    if Routing([x,y],point_dn,points,edge)==True:\n",
    "                        bianli_list.append([point_dn[0],point_dn[1],head,r+1])\n",
    "            head+=1\n",
    "        new_start_point = [bianli_list[head][0],bianli_list[head][1]]\n",
    "        baozang_points.remove(new_start_point)\n",
    "        while(bianli_list[head][2]!=-1):\n",
    "            x0 = bianli_list[head][0]\n",
    "            y0 = bianli_list[head][1]\n",
    "            route.append([x0,y0])\n",
    "            pre_index = bianli_list[head][2]\n",
    "            x1 = bianli_list[pre_index][0]\n",
    "            y1 = bianli_list[pre_index][1]\n",
    "            cv2.line(im_crop, (points[x0][y0][0],points[x0][y0][1]), (points[x1][y1][0],points[x1][y1][1]), (51, 255, 255), 2) # BGR黄色\n",
    "            head = pre_index\n",
    "        route.append([bianli_list[head][0],bianli_list[head][1]])\n",
    "        total_route.append(route[:])\n",
    "        route.clear()\n",
    "        bianli_list.clear()\n",
    "        bianli_list.append([new_start_point[0],new_start_point[1],-1,1])\n",
    "    route = get_route([9,0],new_start_point,im_crop,edge,points)\n",
    "    total_route.append(route)\n",
    "    return im_crop,total_route\n",
    "\n",
    "\n",
    "def find_rect(img):\n",
    "        edge =cv2.Canny(img,200,250);\n",
    "        #binaly_img   = cv2.dilate(edge, kernel, iterations = 1)\n",
    "        contours,hierarchy = cv2.findContours(edge,cv2.RETR_TREE,cv2.CHAIN_APPROX_NONE,)\n",
    "        \n",
    "        #3print(\"-----------------------------\")\n",
    "        #print((contours[2]))\n",
    "        bounding_boxes = [cv2.boundingRect(cnt) for cnt in contours]\n",
    "        index_list = []\n",
    "        #print(contours[2][0])\n",
    "        #找到有内轮廓的轮廓\n",
    "        for i in range (len(bounding_boxes)):\n",
    "             hie = hierarchy[0][i]\n",
    "             j=0;\n",
    "             while(hie[2]>0):\n",
    "                 j+=1\n",
    "                 hie = hierarchy[0][hie[2]]\n",
    "             if j>2:\n",
    "               index_list.append(i)\n",
    "             \n",
    "        for i,box in enumerate(bounding_boxes):\n",
    "            if i in index_list:\n",
    "                if box[2]>1.5*box[3] or box[2]<0.6*box[3] or box[3]*box[2]>10000:\n",
    "                    index_list.remove(i)\n",
    "                \n",
    "                #else:\n",
    "                  #  cv2.rectangle(img, (box[0], box[1]), (box[0] + box[2], box[1] + box[3]), (0, 255, 0), 2)\n",
    "        \n",
    "        nei_list = []    \n",
    "        for i in index_list:\n",
    "             hie = hierarchy[0][i]\n",
    "             j=0;\n",
    "             while(hie[2]>0):   \n",
    "                 hie = hierarchy[0][hie[2]]\n",
    "                 h1 =  bounding_boxes[i][3]\n",
    "             nei_list.append(hie[3])\n",
    "             \n",
    "        nei_list.sort()\n",
    "        \n",
    "        index_list = []\n",
    "        for l in nei_list:\n",
    "            if l not in index_list:\n",
    "                index_list.append(l)\n",
    "                box = bounding_boxes[l]\n",
    "                cv2.rectangle(img, (box[0], box[1]), (box[0] + box[2], box[1] + box[3]), (0, 255, 0), 2)\n",
    "        \n",
    "        points =[]\n",
    "        cv2.imshow(\"img\",img)\n",
    "        #cv2.waitKey(10)\n",
    "        if len(index_list)>=4:\n",
    "             for l in index_list:\n",
    "                x,y,w,h = bounding_boxes[l]\n",
    "                if w*h<400:\n",
    "                    points.append([int(x+w/2),int(y+h/2)])\n",
    "                   \n",
    "        if(len(points)==4):\n",
    "               points.sort()\n",
    "               if(points[0][1]<points[1][1]):\n",
    "                      left_top_p = points[0]\n",
    "                      left_down_p = points[1]\n",
    "               else:\n",
    "                      left_top_p = points[1]\n",
    "                      left_down_p = points[0]\n",
    "                      \n",
    "               \n",
    "               if(points[2][1]<points[3][1]):\n",
    "                      right_top_p = points[2]\n",
    "                      right_down_p = points[3]\n",
    "               else:\n",
    "                      right_top_p = points[3]\n",
    "                      right_down_p = points[2]\n",
    "            \n",
    "               return left_top_p,left_down_p,right_top_p,right_down_p,h1\n",
    "        return 0,0,0,0,0   \n",
    "\n",
    "def img_jiaozheng(left_top_p,left_down_p,right_top_p,right_down_p,frame,h1):\n",
    "       #if (right_down_p[0]-left_top_p[0])>(right_down_p[1]-left_top_p[1]):\n",
    "         #  w = right_down_p[0]-left_top_p[0]\n",
    "       #else:\n",
    "          # w = right_down_p[1]-left_top_p[1]\n",
    "    tl = left_top_p\n",
    "    tr = right_top_p\n",
    "    br = right_down_p\n",
    "    bl = left_down_p\n",
    "    \n",
    "    widthA = np.sqrt(((br[0] - bl[0])**2)+((br[1] - bl[1])**2))\n",
    "    widthB = np.sqrt(((tr[0] - tl[0])**2)+((tr[1] - tl[1])**2))\n",
    "    maxWidth = max(int(widthA),int(widthB))\n",
    "    \n",
    "    heightA = np.sqrt(((tr[0] - br[0])**2)+((tr[1] - br[1])**2))\n",
    "    heightB = np.sqrt(((tl[0] - bl[0])**2)+((tl[1] - bl[1])**2))\n",
    "    maxheight = max(int(heightA),int(heightB))\n",
    "    \n",
    "    target_lf_t = [0,0]\n",
    "    target_rg_t =[maxWidth-1,0]\n",
    "    target_rg_d = [maxWidth-1,maxheight-1]\n",
    "    target_lf_d = [0,maxheight-1]\n",
    "\n",
    "    #开始校正\n",
    "    pointSrc = np.float32([tl, tr, br, bl])  # 原始图像中 4点坐标\n",
    "    pointDst = np.float32([target_lf_t, target_rg_t, target_rg_d, target_lf_d])  # 变换图像中 4点坐标\n",
    "    MP = cv2.getPerspectiveTransform(pointSrc, pointDst)  # 计算投影变换矩阵 M\n",
    "    imgP = cv2.warpPerspective(frame, MP, (maxWidth,maxheight))\n",
    "    im_crop = imgP[int(maxheight/22):maxheight-int(maxheight/22),int(maxWidth/22):maxWidth-int(maxWidth/22),:]     \n",
    "     \n",
    "    cv2.imshow(\"img\",im_crop) #矫正后的图片      \n",
    "    cv2.waitKey(10)\n",
    "          \n",
    "    return im_crop\n",
    "\n",
    "\n",
    "def get_points(im_crop):\n",
    "   gray_image = cv2.cvtColor(im_crop,cv2.COLOR_RGB2GRAY)\n",
    "   ret,edge = cv2.threshold(gray_image,70,255,cv2.THRESH_BINARY_INV)\n",
    "   #edge =cv2.Canny(gray_image,200,250)+edge;\n",
    "   kernel = np.ones((3, 3), np.uint8)\n",
    "   baozang_points = []\n",
    "   points=[]\n",
    "   edge   = cv2.dilate(edge, kernel, iterations = 1)\n",
    "   \n",
    "   kernel = np.ones((1, int(im_crop.shape[1]/30)), np.uint8)\n",
    "   edge1 = cv2.morphologyEx(edge, cv2.MORPH_OPEN, kernel,  iterations=2);\n",
    "   lines = cv2.HoughLines(edge1, 1, np.pi / 2, 80,200)\n",
    "   mid_p_x = []\n",
    "   if 1:\n",
    "       lines1 = lines[:, 0, :]\n",
    "      # mid_p_x = []\n",
    "       for rho, theta in lines1[:]:\n",
    "            a = np.cos(theta)\n",
    "            b = np.sin(theta)  \n",
    "            y0 = b * rho\n",
    "            if y0>0:\n",
    "               mid_p_x.append(y0)\n",
    "\n",
    "   kernel = np.ones((int(im_crop.shape[0]/30), 1), np.uint8)\n",
    "   binaly_img2 = cv2.morphologyEx(edge, cv2.MORPH_OPEN, kernel,  iterations=2);\n",
    "   lines =  cv2.HoughLines(binaly_img2, 1, np.pi / 2, 50,150)\n",
    "   #lines1 = lines[:, 0, :]\n",
    "   mid_p_y = []\n",
    "   if 1:\n",
    "      \n",
    "       lines1 = lines[:, 0, :]\n",
    "       for rho, theta in lines1[:]:\n",
    "            a = np.cos(theta)\n",
    "            b = np.sin(theta)\n",
    "            x0 = a * rho\n",
    "            if x0>0:\n",
    "                mid_p_y.append(x0)\n",
    "   \n",
    "   if(len(mid_p_y)>5 and len(mid_p_x)>5):\n",
    "        mid_p_y.sort()\n",
    "        mid_p_x.sort()\n",
    "        len_x = len(mid_p_x)\n",
    "        len_y = len(mid_p_y)\n",
    "        gap_x=[]\n",
    "        gap_y=[]\n",
    "        for i in range(len_x-1):\n",
    "            gap_x.append(mid_p_x[i+1]-mid_p_x[i])\n",
    "        for i in range(len_y-1):\n",
    "            gap_y.append(mid_p_y[i+1]-mid_p_y[i])\n",
    "        width,height,c =    im_crop.shape\n",
    "        gap_av_x = width/12#np.mean(gap_x)\n",
    "        gap_av_y = height/12#np.mean(gap_y)\n",
    "        remove_x=[]\n",
    "        remove_y=[]\n",
    "        for i,g in enumerate(gap_x):\n",
    "            if g<gap_av_x*0.7:\n",
    "              remove_x.append(mid_p_x[i+1])\n",
    "            if g>gap_av_x*1.5:\n",
    "              mid_p_x.append(mid_p_x[i]+int((mid_p_x[i+1]-mid_p_x[i])/2))\n",
    "        for i ,g in enumerate(gap_y):\n",
    "            if g<gap_av_y*0.7:\n",
    "              remove_y.append(mid_p_y[i+1])\n",
    "            if g>gap_av_y*1.5:\n",
    "              mid_p_y.append(mid_p_y[i]+int((mid_p_y[i+1]-mid_p_y[i])/2))\n",
    "        for x in remove_x:\n",
    "            mid_p_x.remove(x)\n",
    "        for y in remove_y:\n",
    "            mid_p_y.remove(y)\n",
    "        im = im_crop.copy()\n",
    "        for x in mid_p_y:\n",
    "             cv2.line(im, (int(x), 0), (int(x), 600), (0, 0, 255), 2)\n",
    "        for x in mid_p_x:\n",
    "             cv2.line(im, (0, int(x)), (600, int(x)), (0, 0, 255), 2)\n",
    "        \n",
    "        if len(mid_p_x)==11 and len(mid_p_y)==11:\n",
    "            points=[]\n",
    "            mid_p_y.sort();\n",
    "            mid_p_x.sort();\n",
    "            for i in range(10):\n",
    "                x_list = []\n",
    "                y = mid_p_y[i]\n",
    "                for j in range(10):\n",
    "                    x = mid_p_x[j]\n",
    "                    x_list.append([int(y+gap_av_y/2),int(x+gap_av_x/2)])\n",
    "                    #cv2.circle(im_crop, (int(y+gap_av_y/2),int(x+gap_av_x/2)), 3, [0, 0, 255])\n",
    "\n",
    "                points.append(x_list)\n",
    "            kenel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(int(gap_av_y/3),int(gap_av_x/3)))\n",
    "            yuan = cv2.morphologyEx(edge, cv2.MORPH_OPEN, kenel,  iterations=1);\n",
    "            #circles = cv2.HoughCircles(edge, cv2.HOUGH_GRADIENT, 1, 30, param1=100, param2=200, minRadius=0, maxRadius=0)\n",
    "            contours,hierarchy = cv2.findContours(yuan,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)\n",
    "            bounding_boxes = [cv2.boundingRect(cnt) for cnt in contours]\n",
    "            \n",
    "            for bbox in bounding_boxes:\n",
    "                  [x,y,w,h] = bbox\n",
    "                \n",
    "                #baozang_point.append([int(x+w/2),int(y+h/2)])\n",
    "                  if (x>mid_p_y[0]) and ((x+w/2)<mid_p_y[10]) :\n",
    "                    for i in range(10):\n",
    "                        if (x-points[i][0][0])<gap_av_y/2:\n",
    "                            break\n",
    "                          \n",
    "                    for j in range(10):\n",
    "                        if (y-points[0][j][1])<gap_av_x/2:\n",
    "                            baozang_points.append([i,j]);\n",
    "                            break\n",
    "                \n",
    "                    for p in baozang_points:\n",
    "                        x = points[p[0]][p[1]]              \n",
    "                        cv2.circle(im, (x[0],x[1]), 5, [255, 0, 0]) \n",
    "        else:\n",
    "            kenel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(int(gap_av_y/3),int(gap_av_x/3)))\n",
    "            yuan = cv2.morphologyEx(edge, cv2.MORPH_OPEN, kenel,  iterations=1);\n",
    "            \n",
    "      \n",
    "   else:\n",
    "            kenel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(int(30/3),int(30/3)))\n",
    "            yuan = cv2.morphologyEx(edge, cv2.MORPH_OPEN, kenel,  iterations=1);\n",
    "            \n",
    "                    #return binaly_img2+edge1,points,baozang_point\n",
    "                    #cv2.imshow(\"yuan\",yuan)\n",
    "                    #cv2.waitKey(10)\n",
    "   kenel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(3,3))\n",
    "   edge1 = binaly_img2+edge1+cv2.morphologyEx(edge-yuan, cv2.MORPH_OPEN, kenel,  iterations=1);\n",
    "            #edge1 = yuan\n",
    "   #cv2.imshow('shange',im) # 迷宫格\n",
    "   cv2.waitKey(10)\n",
    "   # cv2.imshow(\"edge1\",edge1) #边缘图像\n",
    "   cv2.waitKey(10)        \n",
    "        #cv2.circle(im_crop, (int(y+gap_av_y/2),int(x+gap_av_x/2)), 5, [0, 0, 255])\n",
    "   #edge2=binaly_img2+edge1\n",
    "   return points,baozang_points,edge1\n",
    "            \n",
    "            \n",
    "            #cv2.line(im_crop, (mid_p_y[0], 0), (mid_p_y[0], 600), (0, 0, 255), 2)\n",
    "            \n",
    "            #cv2.line(im_crop, (mid_p_y[10], 0), (mid_p_y[10], 600), (0, 0, 255), 2)\n",
    "   \n",
    "\n",
    "        \n",
    "def del_img():\n",
    "    kernel = np.ones((3, 3), np.uint8)\n",
    "    cap=cv2.VideoCapture(0)  #调用摄像头‘0’一般q是打开电脑自带摄像头，‘1’是打开外部摄像头（只有一个摄像头的情况）\n",
    "    width=800\n",
    "    height=600\n",
    "    cap.set(cv2.CAP_PROP_FRAME_WIDTH,width)#设置图像宽度\n",
    "    cap.set(cv2.CAP_PROP_FRAME_HEIGHT,height)#设置图像高度\n",
    "    #显示图像\n",
    "    while True: \n",
    "        ret,frame=cap.read()#读取图像(frame就是读取的视频帧，对frame处理就是对整个视频的处理)\n",
    "        #print(ret)#\n",
    "        #######例如将图像灰度化处理，\n",
    "        img=cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)#转灰度图\n",
    "        # cv2.imshow(\"imcrop\",img)\n",
    "        input=cv2.waitKey(10)\n",
    "        #gray_image = cv2.medianBlur(img,3)\n",
    "        left_top_p,left_down_p,right_top_p,right_down_p,h1=find_rect(img)\n",
    "        if(left_top_p!=0):    \n",
    "            im_crop = img_jiaozheng(left_top_p,left_down_p,right_top_p,right_down_p,frame,h1)              \n",
    "            cv2.imshow(\"jiaozheng\",im_crop) #矫正后的图片\n",
    "            input=cv2.waitKey(10)\n",
    "            points,baozang_point,edge=get_points(im_crop)\n",
    "            if len(baozang_point)==8:\n",
    "                \n",
    "        \n",
    "                return edge,points,baozang_point,im_crop\n",
    "        #return 0,0,0\n",
    "def route_convert_instruct(route):\n",
    "    instruct_t=[]\n",
    "    instruct = []\n",
    "    instruct.append(3)\n",
    "    pre_dir = [1,0]\n",
    "    for r in route:\n",
    "        instruct = []\n",
    "        length = len(r)\n",
    "        for i in range(2,len(r)):\n",
    "            x = [r[length-i-1][0]-r[length-i][0],r[length-i-1][1]-r[length-i][1]]\n",
    "            if(x==pre_dir):\n",
    "               instruct.append(3)\n",
    "            elif(x[0]<0 and pre_dir[1]>0):\n",
    "               instruct.append(0)\n",
    "            elif(x[0]<0 and pre_dir[1]<0):\n",
    "               instruct.append(1)\n",
    "            elif(x[0]>0 and pre_dir[1]>0):\n",
    "               instruct.append(1)\n",
    "            elif(x[0]>0 and pre_dir[1]<0):\n",
    "               instruct.append(0)\n",
    "            elif(x[1]<0 and pre_dir[0]<0):\n",
    "               instruct.append(0)\n",
    "            elif(x[1]<0 and pre_dir[0]>0):\n",
    "               instruct.append(1)  \n",
    "            elif(x[1]>0 and pre_dir[0]>0):\n",
    "               instruct.append(0)\n",
    "            elif(x[1]>0 and pre_dir[0]<0):\n",
    "               instruct.append(1)\n",
    "            pre_dir = x\n",
    "       # instruct.append(3)\n",
    "        instruct.append(2)\n",
    "        pre_dir =  [-pre_dir[0],-pre_dir[1]]\n",
    "        instruct_t.append(instruct[:])\n",
    "        instruct.clear()\n",
    "    return instruct_t\n",
    "def send_data(ser,ins):\n",
    "    for d in ins:\n",
    "        for i in d:\n",
    "            if(i==0):\n",
    "                ser.write(b'0')#turn right\n",
    "            if(i==1):\n",
    "                ser.write(b'1')#turn left\n",
    "            if(i==2):\n",
    "                ser.write(b'2')#turn back\n",
    "            if(i==3):\n",
    "                ser.write(b'3')#go farward\n",
    "        \n",
    "        ser.write(b'4') #shibiebaozang\n",
    "            \n",
    "    ser.write(b'5')#tingche\n",
    "            \n",
    "def main():\n",
    "    #ser = serial.Serial(\"/dev/ttyAMA0\",115200) #启动串口\n",
    "    #ser.write(b'')\n",
    "    edge,points,baozang_point,im_crop = del_img()\n",
    "    for point in baozang_point:\n",
    "        if(len(baozang_point)==8):\n",
    "            im_crop1,total_r = get_short_route([0,9],baozang_point,im_crop,edge,points)\n",
    "            ins = route_convert_instruct(total_r)\n",
    "            # send_data(ser,ins)  #通过串口发送数据\n",
    "            print(ins)\n",
    "\n",
    "\n",
    "    while(True):\n",
    "        cv2.imshow('lujing',im_crop1) # 路径显示\n",
    "        input=cv2.waitKey(10)\n",
    "        if input==ord('q'):\n",
    "             break\n",
    "    #cap.release()#释放摄像头\n",
    "#cv2.destroyAllWindows()#销毁窗口\n",
    "            \n",
    "if __name__=='__main__':\n",
    "    main()"
   ]
  }
 ],
 "metadata": {
  "language_info": {
   "name": "python"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
